/*!
    \file    gd32f30x_it.c
    \brief   interrupt service routines

    \version 2017-02-10, V1.0.0, firmware for GD32F30x
    \version 2018-10-10, V1.1.0, firmware for GD32F30x
    \version 2018-12-25, V2.0.0, firmware for GD32F30x
    \version 2020-09-30, V2.1.0, firmware for GD32F30x 
*/

/*
    Copyright (c) 2020, GigaDevice Semiconductor Inc.

    Redistribution and use in source and binary forms, with or without modification, 
are permitted provided that the following conditions are met:

    1. Redistributions of source code must retain the above copyright notice, this 
       list of conditions and the following disclaimer.
    2. Redistributions in binary form must reproduce the above copyright notice, 
       this list of conditions and the following disclaimer in the documentation 
       and/or other materials provided with the distribution.
    3. Neither the name of the copyright holder nor the names of its contributors 
       may be used to endorse or promote products derived from this software without 
       specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 
IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 
NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 
OF SUCH DAMAGE.
*/
#include <string.h>
#include "gd32f30x_it.h"
#include "main.h"
#include "spi.h"
#include "pi.h"
#include "svpwm.h"
#include "transforms.h"
#include "smopos.h"

const int16_t SINTable[]=
{
    101,202,302,403,503,604,704,805,905,1006,1106,1207,1307,1408,1508,1608,1709,1809,1910,2010,
	2110,2211,2311,2411,2511,2612,2712,2812,2912,3012,3112,3212,3312,3412,3512,3612,3712,3812,3912,4012,
	4111,4211,4311,4410,4510,4610,4709,4809,4908,5007,5107,5206,5305,5404,5504,5603,5702,5801,5899,5998,
	6097,6196,6295,6393,6492,6590,6689,6787,6885,6984,7082,7180,7278,7376,7474,7572,7670,7767,7865,7962,
	8060,8157,8255,8352,8449,8546,8643,8740,8837,8934,9030,9127,9224,9320,9416,9513,9609,9705,9801,9897,
	9992,10088,10184,10279,10375,10470,10565,10660,10755,10850,10945,11040,11134,11229,11323,11417,11512,11606,11700,11794,
	11887,11981,12074,12168,12261,12354,12447,12540,12633,12726,12818,12911,13003,13095,13187,13279,13371,13463,13555,13646,
	13737,13829,13920,14011,14101,14192,14283,14373,14463,14553,14643,14733,14823,14913,15002,15091,15181,15270,15358,15447,
	15536,15624,15713,15801,15889,15977,16064,16152,16239,16326,16414,16500,16587,16674,16760,16847,16933,17019,17105,17190,
	17276,17361,17446,17531,17616,17701,17785,17870,17954,18038,18122,18205,18289,18372,18455,18538,18621,18704,18786,18869,
	18951,19033,19114,19196,19277,19359,19440,19520,19601,19682,19762,19842,19922,20002,20081,20160,20240,20319,20397,20476,
	20554,20632,20710,20788,20866,20943,21021,21098,21174,21251,21327,21404,21480,21556,21631,21707,21782,21857,21932,22006,
	22081,22155,22229,22302,22376,22449,22522,22595,22668,22741,22813,22885,22957,23028,23100,23171,23242,23313,23383,23454,
	23524,23594,23663,23733,23802,23871,23940,24008,24076,24145,24212,24280,24347,24415,24481,24548,24615,24681,24747,24813,
	24878,24944,25009,25074,25138,25202,25267,25331,25394,25458,25521,25584,25646,25709,25771,25833,25895,25956,26017,26078,
	26139,26200,26260,26320,26380,26439,26499,26558,26616,26675,26733,26791,26849,26906,26964,27021,27077,27134,27190,27246,
	27302,27357,27412,27467,27522,27577,27631,27685,27738,27792,27845,27898,27950,28003,28055,28107,28158,28209,28260,28311,
	28362,28412,28462,28512,28561,28610,28659,28708,28756,28804,28852,28899,28947,28994,29040,29087,29133,29179,29224,29270,
	29315,29360,29404,29448,29492,29536,29579,29622,29665,29708,29750,29792,29834,29875,29916,29957,29998,30038,30078,30118,
	30157,30197,30236,30274,30313,30351,30388,30426,30463,30500,30536,30573,30609,30645,30680,30715,30750,30785,30819,30853,
	30887,30920,30953,30986,31019,31051,31083,31115,31146,31177,31208,31238,31269,31299,31328,31358,31387,31415,31444,31472,
	31500,31527,31555,31582,31608,31635,31661,31686,31712,31737,31762,31786,31811,31835,31858,31882,31905,31928,31950,31972,
	31994,32016,32037,32058,32079,32099,32119,32139,32158,32177,32196,32215,32233,32251,32269,32286,32303,32320,32336,32352,
	32368,32384,32399,32414,32428,32443,32457,32470,32484,32497,32510,32522,32534,32546,32558,32569,32580,32590,32601,32611,
	32620,32630,32639,32648,32656,32664,32672,32680,32687,32694,32701,32707,32713,32719,32724,32729,32734,32738,32742,32746,
	32750,32753,32756,32759,32761,32763,32765,32766,32767,32767,32767,32767,32767,32767,32767,32766,32765,32763,32761,32759,
	32756,32753,32750,32746,32742,32738,32734,32729,32724,32719,32713,32707,32701,32694,32687,32680,32672,32664,32656,32648,
	32639,32630,32620,32611,32601,32590,32580,32569,32558,32546,32534,32522,32510,32497,32484,32470,32457,32443,32428,32414,
	32399,32384,32368,32352,32336,32320,32303,32286,32269,32251,32233,32215,32196,32177,32158,32139,32119,32099,32079,32058,
	32037,32016,31994,31972,31950,31928,31905,31882,31858,31835,31811,31786,31762,31737,31712,31686,31661,31635,31608,31582,
	31555,31527,31500,31472,31444,31415,31387,31358,31328,31299,31269,31238,31208,31177,31146,31115,31083,31051,31019,30986,
	30953,30920,30887,30853,30819,30785,30750,30715,30680,30645,30609,30573,30536,30500,30463,30426,30388,30351,30313,30274,
	30236,30197,30157,30118,30078,30038,29998,29957,29916,29875,29834,29792,29750,29708,29665,29622,29579,29536,29492,29448,
	29404,29360,29315,29270,29224,29179,29133,29087,29040,28994,28947,28899,28852,28804,28756,28708,28659,28610,28561,28512,
	28462,28412,28362,28311,28260,28209,28158,28107,28055,28003,27950,27898,27845,27792,27738,27685,27631,27577,27522,27467,
	27412,27357,27302,27246,27190,27134,27077,27021,26964,26906,26849,26791,26733,26675,26616,26558,26499,26439,26380,26320,
	26260,26200,26139,26078,26017,25956,25895,25833,25771,25709,25646,25584,25521,25458,25394,25331,25267,25202,25138,25074,
	25009,24944,24878,24813,24747,24681,24615,24548,24481,24415,24347,24280,24212,24145,24076,24008,23940,23871,23802,23733,
	23663,23594,23524,23454,23383,23313,23242,23171,23100,23028,22957,22885,22813,22741,22668,22595,22522,22449,22376,22302,
	22229,22155,22081,22006,21932,21857,21782,21707,21631,21556,21480,21404,21327,21251,21174,21098,21021,20943,20866,20788,
	20710,20632,20554,20476,20397,20319,20240,20160,20081,20002,19922,19842,19762,19682,19601,19520,19440,19359,19277,19196,
	19114,19033,18951,18869,18786,18704,18621,18538,18455,18372,18289,18205,18122,18038,17954,17870,17785,17701,17616,17531,
	17446,17361,17276,17190,17105,17019,16933,16847,16760,16674,16587,16500,16414,16326,16239,16152,16064,15977,15889,15801,
	15713,15624,15536,15447,15358,15270,15181,15091,15002,14913,14823,14733,14643,14553,14463,14373,14283,14192,14101,14011,
	13920,13829,13737,13646,13555,13463,13371,13279,13187,13095,13003,12911,12818,12726,12633,12540,12447,12354,12261,12168,
	12074,11981,11887,11794,11700,11606,11512,11417,11323,11229,11134,11040,10945,10850,10755,10660,10565,10470,10375,10279,
	10184,10088,9992,9897,9801,9705,9609,9513,9416,9320,9224,9127,9030,8934,8837,8740,8643,8546,8449,8352,
	8255,8157,8060,7962,7865,7767,7670,7572,7474,7376,7278,7180,7082,6984,6885,6787,6689,6590,6492,6393,
	6295,6196,6097,5998,5899,5801,5702,5603,5504,5404,5305,5206,5107,5007,4908,4809,4709,4610,4510,4410,
	4311,4211,4111,4012,3912,3812,3712,3612,3512,3412,3312,3212,3112,3012,2912,2812,2712,2612,2511,2411,
	2311,2211,2110,2010,1910,1809,1709,1608,1508,1408,1307,1207,1106,1006,905,805,704,604,503,403,
	302,202,101,1,-100,-201,-301,-402,-502,-603,-703,-804,-904,-1005,-1105,-1206,-1306,-1407,-1507,-1607,
	-1708,-1808,-1909,-2009,-2109,-2210,-2310,-2410,-2510,-2611,-2711,-2811,-2911,-3011,-3111,-3211,-3311,-3411,-3511,-3611,
	-3711,-3811,-3911,-4011,-4110,-4210,-4310,-4409,-4509,-4609,-4708,-4808,-4907,-5006,-5106,-5205,-5304,-5403,-5503,-5602,
	-5701,-5800,-5898,-5997,-6096,-6195,-6294,-6392,-6491,-6589,-6688,-6786,-6884,-6983,-7081,-7179,-7277,-7375,-7473,-7571,
	-7669,-7766,-7864,-7961,-8059,-8156,-8254,-8351,-8448,-8545,-8642,-8739,-8836,-8933,-9029,-9126,-9223,-9319,-9415,-9512,
	-9608,-9704,-9800,-9896,-9991,-10087,-10183,-10278,-10374,-10469,-10564,-10659,-10754,-10849,-10944,-11039,-11133,-11228,-11322,-11416,
	-11511,-11605,-11699,-11793,-11886,-11980,-12073,-12167,-12260,-12353,-12446,-12539,-12632,-12725,-12817,-12910,-13002,-13094,-13186,-13278,
	-13370,-13462,-13554,-13645,-13736,-13828,-13919,-14010,-14100,-14191,-14282,-14372,-14462,-14552,-14642,-14732,-14822,-14912,-15001,-15090,
	-15180,-15269,-15357,-15446,-15535,-15623,-15712,-15800,-15888,-15976,-16063,-16151,-16238,-16325,-16413,-16499,-16586,-16673,-16759,-16846,
	-16932,-17018,-17104,-17189,-17275,-17360,-17445,-17530,-17615,-17700,-17784,-17869,-17953,-18037,-18121,-18204,-18288,-18371,-18454,-18537,
	-18620,-18703,-18785,-18868,-18950,-19032,-19113,-19195,-19276,-19358,-19439,-19519,-19600,-19681,-19761,-19841,-19921,-20001,-20080,-20159,
	-20239,-20318,-20396,-20475,-20553,-20631,-20709,-20787,-20865,-20942,-21020,-21097,-21173,-21250,-21326,-21403,-21479,-21555,-21630,-21706,
	-21781,-21856,-21931,-22005,-22080,-22154,-22228,-22301,-22375,-22448,-22521,-22594,-22667,-22740,-22812,-22884,-22956,-23027,-23099,-23170,
	-23241,-23312,-23382,-23453,-23523,-23593,-23662,-23732,-23801,-23870,-23939,-24007,-24075,-24144,-24211,-24279,-24346,-24414,-24480,-24547,
	-24614,-24680,-24746,-24812,-24877,-24943,-25008,-25073,-25137,-25201,-25266,-25330,-25393,-25457,-25520,-25583,-25645,-25708,-25770,-25832,
	-25894,-25955,-26016,-26077,-26138,-26199,-26259,-26319,-26379,-26438,-26498,-26557,-26615,-26674,-26732,-26790,-26848,-26905,-26963,-27020,
	-27076,-27133,-27189,-27245,-27301,-27356,-27411,-27466,-27521,-27576,-27630,-27684,-27737,-27791,-27844,-27897,-27949,-28002,-28054,-28106,
	-28157,-28208,-28259,-28310,-28361,-28411,-28461,-28511,-28560,-28609,-28658,-28707,-28755,-28803,-28851,-28898,-28946,-28993,-29039,-29086,
	-29132,-29178,-29223,-29269,-29314,-29359,-29403,-29447,-29491,-29535,-29578,-29621,-29664,-29707,-29749,-29791,-29833,-29874,-29915,-29956,
	-29997,-30037,-30077,-30117,-30156,-30196,-30235,-30273,-30312,-30350,-30387,-30425,-30462,-30499,-30535,-30572,-30608,-30644,-30679,-30714,
	-30749,-30784,-30818,-30852,-30886,-30919,-30952,-30985,-31018,-31050,-31082,-31114,-31145,-31176,-31207,-31237,-31268,-31298,-31327,-31357,
	-31386,-31414,-31443,-31471,-31499,-31526,-31554,-31581,-31607,-31634,-31660,-31685,-31711,-31736,-31761,-31785,-31810,-31834,-31857,-31881,
	-31904,-31927,-31949,-31971,-31993,-32015,-32036,-32057,-32078,-32098,-32118,-32138,-32157,-32176,-32195,-32214,-32232,-32250,-32268,-32285,
	-32302,-32319,-32335,-32351,-32367,-32383,-32398,-32413,-32427,-32442,-32456,-32469,-32483,-32496,-32509,-32521,-32533,-32545,-32557,-32568,
	-32579,-32589,-32600,-32610,-32619,-32629,-32638,-32647,-32655,-32663,-32671,-32679,-32686,-32693,-32700,-32706,-32712,-32718,-32723,-32728,
	-32733,-32737,-32741,-32745,-32749,-32752,-32755,-32758,-32760,-32762,-32764,-32765,-32766,-32767,-32767,-32767,-32767,-32767,-32766,-32765,
	-32764,-32762,-32760,-32758,-32755,-32752,-32749,-32745,-32741,-32737,-32733,-32728,-32723,-32718,-32712,-32706,-32700,-32693,-32686,-32679,
	-32671,-32663,-32655,-32647,-32638,-32629,-32619,-32610,-32600,-32589,-32579,-32568,-32557,-32545,-32533,-32521,-32509,-32496,-32483,-32469,
	-32456,-32442,-32427,-32413,-32398,-32383,-32367,-32351,-32335,-32319,-32302,-32285,-32268,-32250,-32232,-32214,-32195,-32176,-32157,-32138,
	-32118,-32098,-32078,-32057,-32036,-32015,-31993,-31971,-31949,-31927,-31904,-31881,-31857,-31834,-31810,-31785,-31761,-31736,-31711,-31685,
	-31660,-31634,-31607,-31581,-31554,-31526,-31499,-31471,-31443,-31414,-31386,-31357,-31327,-31298,-31268,-31237,-31207,-31176,-31145,-31114,
	-31082,-31050,-31018,-30985,-30952,-30919,-30886,-30852,-30818,-30784,-30749,-30714,-30679,-30644,-30608,-30572,-30535,-30499,-30462,-30425,
	-30387,-30350,-30312,-30273,-30235,-30196,-30156,-30117,-30077,-30037,-29997,-29956,-29915,-29874,-29833,-29791,-29749,-29707,-29664,-29621,
	-29578,-29535,-29491,-29447,-29403,-29359,-29314,-29269,-29223,-29178,-29132,-29086,-29039,-28993,-28946,-28898,-28851,-28803,-28755,-28707,
	-28658,-28609,-28560,-28511,-28461,-28411,-28361,-28310,-28259,-28208,-28157,-28106,-28054,-28002,-27949,-27897,-27844,-27791,-27737,-27684,
	-27630,-27576,-27521,-27466,-27411,-27356,-27301,-27245,-27189,-27133,-27076,-27020,-26963,-26905,-26848,-26790,-26732,-26674,-26615,-26557,
	-26498,-26438,-26379,-26319,-26259,-26199,-26138,-26077,-26016,-25955,-25894,-25832,-25770,-25708,-25645,-25583,-25520,-25457,-25393,-25330,
	-25266,-25201,-25137,-25073,-25008,-24943,-24877,-24812,-24746,-24680,-24614,-24547,-24480,-24414,-24346,-24279,-24211,-24144,-24075,-24007,
	-23939,-23870,-23801,-23732,-23662,-23593,-23523,-23453,-23382,-23312,-23241,-23170,-23099,-23027,-22956,-22884,-22812,-22740,-22667,-22594,
	-22521,-22448,-22375,-22301,-22228,-22154,-22080,-22005,-21931,-21856,-21781,-21706,-21630,-21555,-21479,-21403,-21326,-21250,-21173,-21097,
	-21020,-20942,-20865,-20787,-20709,-20631,-20553,-20475,-20396,-20318,-20239,-20159,-20080,-20001,-19921,-19841,-19761,-19681,-19600,-19519,
	-19439,-19358,-19276,-19195,-19113,-19032,-18950,-18868,-18785,-18703,-18620,-18537,-18454,-18371,-18288,-18204,-18121,-18037,-17953,-17869,
	-17784,-17700,-17615,-17530,-17445,-17360,-17275,-17189,-17104,-17018,-16932,-16846,-16759,-16673,-16586,-16499,-16413,-16325,-16238,-16151,
	-16063,-15976,-15888,-15800,-15712,-15623,-15535,-15446,-15357,-15269,-15180,-15090,-15001,-14912,-14822,-14732,-14642,-14552,-14462,-14372,
	-14282,-14191,-14100,-14010,-13919,-13828,-13736,-13645,-13554,-13462,-13370,-13278,-13186,-13094,-13002,-12910,-12817,-12725,-12632,-12539,
	-12446,-12353,-12260,-12167,-12073,-11980,-11886,-11793,-11699,-11605,-11511,-11416,-11322,-11228,-11133,-11039,-10944,-10849,-10754,-10659,
	-10564,-10469,-10374,-10278,-10183,-10087,-9991,-9896,-9800,-9704,-9608,-9512,-9415,-9319,-9223,-9126,-9029,-8933,-8836,-8739,
	-8642,-8545,-8448,-8351,-8254,-8156,-8059,-7961,-7864,-7766,-7669,-7571,-7473,-7375,-7277,-7179,-7081,-6983,-6884,-6786,
	-6688,-6589,-6491,-6392,-6294,-6195,-6096,-5997,-5898,-5800,-5701,-5602,-5503,-5403,-5304,-5205,-5106,-5006,-4907,-4808,
	-4708,-4609,-4509,-4409,-4310,-4210,-4110,-4011,-3911,-3811,-3711,-3611,-3511,-3411,-3311,-3211,-3111,-3011,-2911,-2811,
	-2711,-2611,-2510,-2410,-2310,-2210,-2109,-2009,-1909,-1808,-1708,-1607,-1507,-1407,-1306,-1206,-1105,-1005,-904,-804,
	-703,-603,-502,-402,-301,-201,-100,0
};

uint16_t adcValue[3];

SVGEN svpwmObj = SVGEN_DEFAULTS;
CLARKE clarkeObj = CLARKE_DEFAULTS;
PARK parkObj = PARK_DEFAULTS;
IPARK iparkObj = IPARK_DEFAULTS;
//SMOPOS smoposObj = SMOPOS_DEFAULTS;

int32_t theta=0, thetaLast;
int32_t thetaTmp = 0, posTmp = 0;
uint16_t pwmValue[3]={1500,1500,1500};
int32_t uqTemp = 0, udTemp = 0;
uint32_t pos;
int32_t speed,speedTmp;
int32_t speedTarget = 0;

can_receive_message_struct  rxMsg;
can_trasnmit_message_struct txMsg;
/*!
    \brief      this function handles NMI exception
    \param[in]  none
    \param[out] none
    \retval     none
*/
void NMI_Handler(void)
{
}

/*!
    \brief      this function handles HardFault exception
    \param[in]  none
    \param[out] none
    \retval     none
*/
void HardFault_Handler(void)
{
    /* if Hard Fault exception occurs, go to infinite loop */
    while (1){
    }
}

/*!
    \brief      this function handles MemManage exception
    \param[in]  none
    \param[out] none
    \retval     none
*/
void MemManage_Handler(void)
{
    /* if Memory Manage exception occurs, go to infinite loop */
    while (1){
    }
}

/*!
    \brief      this function handles BusFault exception
    \param[in]  none
    \param[out] none
    \retval     none
*/
void BusFault_Handler(void)
{
    /* if Bus Fault exception occurs, go to infinite loop */
    while (1){
    }
}

/*!
    \brief      this function handles UsageFault exception
    \param[in]  none
    \param[out] none
    \retval     none
*/
void UsageFault_Handler(void)
{
    /* if Usage Fault exception occurs, go to infinite loop */
    while (1){
    }
}

/*!
    \brief      this function handles SVC exception
    \param[in]  none
    \param[out] none
    \retval     none
*/
void SVC_Handler(void)
{
}

/*!
    \brief      this function handles DebugMon exception
    \param[in]  none
    \param[out] none
    \retval     none
*/
void DebugMon_Handler(void)
{
}

/*!
    \brief      this function handles PendSV exception
    \param[in]  none
    \param[out] none
    \retval     none
*/
void PendSV_Handler(void)
{
}

/*!
    \brief      this function handles SysTick exception
    \param[in]  none
    \param[out] none
    \retval     none
*/
void SysTick_Handler(void)
{
}

/*!
    \brief      Timer1 1s中断  周期CAN发送
    \param[in]  none
    \param[out] none
    \retval     none
*/
void TIMER1_IRQHandler(void)
{
	if(timer_interrupt_flag_get(TIMER1, TIMER_INT_FLAG_UP)!=RESET)
	{
		timer_interrupt_flag_clear(TIMER1, TIMER_INT_FLAG_UP);
		
		txMsg.tx_sfid = 0x201;
		txMsg.tx_efid = 0x00;
		txMsg.tx_ft = CAN_FT_DATA;
		txMsg.tx_ff = CAN_FF_STANDARD;
		txMsg.tx_dlen = 8;
		txMsg.tx_data[0] = theta;
		txMsg.tx_data[1] = theta>>8;
		txMsg.tx_data[2] = speed;
		txMsg.tx_data[3] = speed>>8;
		
		
		can_message_transmit(CAN0, &txMsg);
	}
}
/*!
    \brief      CAN 接受中断  接受CAN数据
    \param[in]  none
    \param[out] none
    \retval     none
*/
void USBD_LP_CAN0_RX0_IRQHandler(void)
{
	memset(rxMsg.rx_data, 0, sizeof(rxMsg.rx_data));
	can_message_receive(CAN0, CAN_FIFO0, &rxMsg);
	
	if(rxMsg.rx_sfid == 0x21)
	{
		speedTarget = (rxMsg.rx_data[1]<<8 | rxMsg.rx_data[0])-0x8000;
	}
}
/*!
    \brief      Timer0 载波中断  20kHz 50us
    \param[in]  none
    \param[out] none
    \retval     none
*/
void TIMER0_UP_IRQHandler(void)
{
	int32_t sinValue = 0,cosValue = 32768;
	if(timer_interrupt_flag_get(TIMER0, TIMER_INT_FLAG_UP)!=RESET)
	{
		timer_interrupt_flag_clear(TIMER0, TIMER_INT_FLAG_UP);
		
		/* SPI 读取磁编位置 */
		EnconderReadData(&pos);
		
//		sinValue = SINTable[(smoposObj.Theta>>5)%2048];
//		cosValue = SINTable[((smoposObj.Theta>>5)+512)%2048];
//		
//		smoposObj.cosine = cosValue;
//		smoposObj.sine = sinValue;
//		smoposObj.Fsmopos = 1;
//		smoposObj.Gsmopos = 1;
//		smoposObj.Kslide = 1;
//		smoposObj.Kslf = 1;
//		smoposObj.kp = 1;
//		smoposObj.ki = 1;
//		smoposObj.Valpha = iparkObj.Alpha;
//		smoposObj.Vbeta = iparkObj.Beta;
//		smoposObj.Ialpha = clarkeObj.Alpha;
//		smoposObj.Ibeta = clarkeObj.Beta;
//		SMO_Run(&smoposObj);
		
		thetaLast = theta;
		/* 根据磁编反馈位置计算角度 */
		posTmp = pos - 2095;
		if(posTmp < 0)
		{
			posTmp += 16384;
		}		
		thetaTmp = (posTmp)%(2340);
		theta = 2048-((thetaTmp*7)>>3);
		
		/* 根据磁编反馈角度计算速度 */
		if((theta - thetaLast) > 1024)
		{
			speedTmp = theta - thetaLast -2048;
		}
		else if((theta - thetaLast) < -1024)
		{
			speedTmp = theta - thetaLast + 2048;
		}
		else
		{
			speedTmp = theta - thetaLast;
		}
		speed = (speedTmp*1000000)>>16;
		
		/* 更新正余弦值 */
		sinValue = SINTable[(theta)%2048];
		cosValue = SINTable[((theta)+512)%2048];
		
		/* 电流Clarke变换 */
		clarkeObj.Bs = -(adcValue[1]-2155)<<6;
		clarkeObj.Cs = -(adcValue[2]-2155)<<6;
		clarkeObj.As = -clarkeObj.Bs - clarkeObj.Cs;
		CLARKE_Run(&clarkeObj);
		
		/* 电流Park变换 */
		parkObj.Alpha = clarkeObj.Alpha;
		parkObj.Beta = clarkeObj.Beta;
		parkObj.Cosine = cosValue;
		parkObj.Sine = sinValue;
		PARK_Run(&parkObj);
		
		/* 速度环 */
		piSpeed.Ref = speedTarget;
		piSpeed.Fdb = speed;
		PI_Run(&piSpeed);
		
		piIq.Ref = piSpeed.Out;
		piIq.Fdb = parkObj.Qs;
		PI_Run(&piIq);
		
		piId.Ref = 0;
		piId.Fdb = parkObj.Ds;
		PI_Run(&piId);
		
		/* 参考电压Ud Uq反Park变化计算Ualpha Ubeta */
		iparkObj.Ds = piId.Out;
		iparkObj.Qs = _IQ15mpy(piIq.Out,15000);
		iparkObj.Cosine = cosValue;
		iparkObj.Sine = sinValue;
		iPARK_Run(&iparkObj);
		
		/* svpwm矢量占空比计算 */
		svpwmObj.Ualpha = iparkObj.Alpha;
		svpwmObj.Ubeta  = iparkObj.Beta;
		SVPWM_Run(&svpwmObj);
		
		/* 更新输出占空比 */
		pwmValue[0] = ((svpwmObj.Ta*1500)>>15)+1500;
		pwmValue[1] = ((svpwmObj.Tb*1500)>>15)+1500;
		pwmValue[2] = ((svpwmObj.Tc*1500)>>15)+1500;
		
		/* 更新TIMER0 比较寄存器 */
		timer_channel_output_pulse_value_config(TIMER0,TIMER_CH_0,pwmValue[0]);
		timer_channel_output_pulse_value_config(TIMER0,TIMER_CH_1,pwmValue[1]);
		timer_channel_output_pulse_value_config(TIMER0,TIMER_CH_2,pwmValue[2]);
    }
}
/*!
    \brief      ADC中断  获取电流采样值和母线电压采样值
    \param[in]  none
    \param[out] none
    \retval     none
*/
void ADC0_1_IRQHandler(void)
{
	adc_interrupt_flag_clear(ADC0, ADC_INT_EOIC);
	/* 母线电压采样 */
	adcValue[0] = adc_inserted_data_read(ADC0, ADC_INSERTED_CHANNEL_0);
	/* BC相电流采样 */
	adcValue[1] = adc_inserted_data_read(ADC0, ADC_INSERTED_CHANNEL_1);
	adcValue[2] = adc_inserted_data_read(ADC0, ADC_INSERTED_CHANNEL_2);
}
